
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_auto.c
  * @author     baiyang
  * @date       2022-10-1
  ******************************************************************************
  */

/*
 * Init and run calls for auto flight mode
 *
 * This file contains the implementation for Land, Waypoint navigation and Takeoff from Auto mode
 * Command execution code (i.e. command_logic.pde) should:
 *      a) switch to Auto flight mode with set_mode() function.  This will cause auto_init to be called
 *      b) call one of the three auto initialisation functions: auto_wp_start(), auto_takeoff_start(), auto_land_start()
 *      c) call one of the verify functions auto_wp_verify(), auto_takeoff_verify, auto_land_verify repeated to check if the command has completed
 * The main loop (i.e. fast loop) will call update_flight_modes() which will in turn call auto_run() which, based upon the auto_mode variable will call
 *      correct auto_wp_run, auto_takeoff_run or auto_land_run to actually implement the feature
 */

/*
 *  While in the auto flight mode, navigation or do/now commands can be run.
 *  Code in this file implements the navigation commands
 */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"

#include <rtc/gp_rtc.h>
#include <gcs_mavlink/gcs.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool mauto_init(mode_base_t mode, bool ignore_checks);
static void mauto_exit(mode_base_t mode);
static void mauto_run(mode_base_t mode);
static ModeNumber mauto_mode_number(mode_base_t mode);
static bool mauto_requires_GPS(mode_base_t mode);
static bool mauto_has_manual_throttle(mode_base_t mode);
static bool mauto_allows_arming(mode_base_t mode, enum ArmingMethod method);
static bool mauto_is_autopilot(mode_base_t mode);
static bool mauto_in_guided_mode(mode_base_t mode);
static bool mauto_requires_terrain_failsafe(mode_base_t mode);
static bool mauto_has_user_takeoff(mode_base_t mode, bool must_navigate);
static const char *mauto_name(mode_base_t mode);
static const char *mauto_name4(mode_base_t mode);
static uint32_t mauto_wp_distance(mode_base_t mode);
static int32_t mauto_wp_bearing(mode_base_t mode);
static float mauto_crosstrack_error(mode_base_t mode);
static bool mauto_get_wp(mode_base_t mode, Location *loc);
static bool mauto_pause(mode_base_t mode);
static bool mauto_resume(mode_base_t mode);
static bool mauto_is_landing(mode_base_t mode);
static bool mauto_is_taking_off(mode_base_t mode);
static bool mauto_use_pilot_yaw(mode_base_t mode);
static bool mauto_set_speed_xy(mode_base_t mode, float speed_xy_cms);
static bool mauto_set_speed_up(mode_base_t mode, float speed_up_cms);
static bool mauto_set_speed_down(mode_base_t mode, float speed_down_cms);

static bool mauto_start_command(const mission_command_t cmd);
static bool mauto_verify_command(const mission_command_t cmd);
static void mauto_exit_mission();

static void mauto_takeoff_run(mode_base_t mode);
static void mauto_loiter_run(mode_base_t mode);
static void mauto_wp_run(mode_base_t mode);
static void mauto_land_run(mode_base_t mode);
static void mauto_rtl_run(mode_base_t mode);
static void mauto_loiter_to_alt_run(mode_base_t mode);
#if NAV_GUIDED == ENABLED
static void mauto_nav_guided_run(mode_base_t mode);
#endif
static void mauto_nav_attitude_time_run(mode_base_t mode);

static bool mauto_shift_alt_to_current_alt(mode_base_t mode, Location* target_loc);

static void mauto_do_takeoff(ModeAuto* mode_auto, const mission_command_t cmd);
static Location mauto_loc_from_cmd(const mission_command_t cmd, const Location* default_loc);
static void mauto_do_nav_wp(ModeAuto* mode_auto, const mission_command_t cmd);
static bool mauto_set_next_wp(mode_base_t mode, const mission_command_t current_cmd, const Location *default_loc);
static void mauto_get_spline_from_cmd(const mission_command_t cmd, const Location* default_loc, Location* dest_loc, Location* next_dest_loc, bool* next_dest_loc_is_spline);
static void mauto_do_land(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_RTL(ModeAuto* mode_auto);
static void mauto_do_loiter_unlimited(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_loiter_time(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_loiter_to_alt(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_spline_wp(ModeAuto* mode_auto, const mission_command_t cmd);
#if NAV_GUIDED == ENABLED
static void mauto_do_nav_guided_enable(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_guided_limits(ModeAuto* mode_auto, const mission_command_t cmd);
#endif
static void mauto_do_nav_delay(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_nav_attitude_time(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_wait_delay(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_within_distance(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_yaw(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_change_speed(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_set_home(ModeAuto* mode_auto, const mission_command_t cmd);
static void mauto_do_roi(ModeAuto* mode_auto, const mission_command_t cmd);

static bool mauto_verify_takeoff(mode_base_t mode);
static bool mauto_verify_nav_wp(mode_base_t mode, const mission_command_t cmd);
static bool mauto_verify_land(mode_base_t mode);
static bool mauto_verify_RTL(mode_base_t mode);
static bool mauto_verify_loiter_unlimited(mode_base_t mode);
static bool mauto_verify_loiter_time(mode_base_t mode, const mission_command_t cmd);
static bool mauto_verify_loiter_to_alt(mode_base_t mode);
static bool mauto_verify_spline_wp(mode_base_t mode, const mission_command_t cmd);
#if NAV_GUIDED == ENABLED
static bool mauto_verify_nav_guided_enable(mode_base_t mode, const mission_command_t cmd);
#endif
static bool mauto_verify_nav_delay(mode_base_t mode, const mission_command_t cmd);
static bool mauto_verify_nav_attitude_time(mode_base_t mode, const mission_command_t cmd);
static bool mauto_verify_wait_delay(mode_base_t mode);
static bool mauto_verify_within_distance(mode_base_t mode);
static bool mauto_verify_yaw(mode_base_t mode);
/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_auto_ops = {
        .mode_number = mauto_mode_number,
        .init        = mauto_init,
        .exit        = mauto_exit,
        .run         = mauto_run,
        .requires_GPS = mauto_requires_GPS,
        .has_manual_throttle = mauto_has_manual_throttle,
        .allows_arming = mauto_allows_arming,
        .is_autopilot = mauto_is_autopilot,
        .has_user_takeoff = mauto_has_user_takeoff,
        .in_guided_mode = mauto_in_guided_mode,
        .logs_attitude = NULL,
        .allows_save_trim = NULL,
        .allows_autotune = NULL,
        .allows_flip = NULL,
        .name = mauto_name,
        .name4 = mauto_name4,
        .is_taking_off = mauto_is_taking_off,
        .is_landing = mauto_is_landing,
        .requires_terrain_failsafe = mauto_requires_terrain_failsafe,
        .get_wp = mauto_get_wp,
        .wp_bearing = mauto_wp_bearing,
        .wp_distance = mauto_wp_distance,
        .crosstrack_error = mauto_crosstrack_error,
        .output_to_motors = NULL,
        .use_pilot_yaw = mauto_use_pilot_yaw,
        .throttle_hover = NULL,
        .do_user_takeoff_start = NULL,
        .set_speed_xy = mauto_set_speed_xy,
        .set_speed_up = mauto_set_speed_up,
        .set_speed_down = mauto_set_speed_down,
        .pause = mauto_pause,
        .resume = mauto_resume};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/********************************************************************************/
// Public function
/********************************************************************************/

void mode_auto_ctor(ModeAuto* mode_auto)
{
    mode_ctor(&mode_auto->mode, &mode_auto_ops);

    mode_auto->auto_mode = AUTO_SUB_TAKEOFF;
    mode_auto->state     = AutoFlyToLocation;

    mode_auto->nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;

    mission_init(mauto_start_command, mauto_verify_command, mauto_exit_mission);
}

// set submode.  This may re-trigger the vehicle's EKF failsafe if the new submode requires a position estimate
void mauto_set_submode(ModeAuto* mode_auto, enum AutoSubMode new_submode)
{
    // return immediately if the submode has not been changed
    if (new_submode == mode_auto->auto_mode) {
        return;
    }

    // backup old mode
    enum AutoSubMode old_submode = mode_auto->auto_mode;

    // set mode
    mode_auto->auto_mode = new_submode;

    // if changing out of the nav-attitude-time submode, recheck the EKF failsafe
    // this may trigger a flight mode change if the EKF failsafe is active
    if (old_submode == AUTO_SUB_NAV_ATTITUDE_TIME) {
        //copter.failsafe_ekf_recheck();
    }
}

/********************************************************************************/
// Start Nav (Must) commands
/********************************************************************************/
// initialise waypoint controller to implement take-off
void mauto_takeoff_start(ModeAuto* mode_auto, const Location* dest_loc)
{
    mode_base_t mode = &mode_auto->mode;

    if (!location_initialised(&fms.current_loc)) {
        // this should never happen because mission commands are not executed until
        // the AHRS/EKF origin is set by which time current_loc should also have been set
        //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        return;
    }

    // calculate current and target altitudes
    // by default current_alt_cm and alt_target_cm are alt-above-EKF-origin
    int32_t alt_target_cm;
    bool alt_target_terrain = false;
    float current_alt_cm = ahrs_get_position_z_up_cm(fms.ahrs);
    float terrain_offset;   // terrain's altitude in cm above the ekf origin
    if ((location_get_alt_frame(dest_loc) == ALT_FRAME_ABOVE_TERRAIN) && wpnav_get_terrain_offset(mode->wp_nav, &terrain_offset)) {
        // subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain
        current_alt_cm -= terrain_offset;

        // specify alt_target_cm as alt-above-terrain
        alt_target_cm = dest_loc->alt;
        alt_target_terrain = true;
    } else {
        // set horizontal target
        Location dest = *dest_loc;
        dest.lat = fms.current_loc.lat;
        dest.lng = fms.current_loc.lng;

        // get altitude target above EKF origin
        if (!location_get_alt_cm(&dest, ALT_FRAME_ABOVE_ORIGIN, &alt_target_cm)) {
            // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
            //AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
            // fall back to altitude above current altitude
            alt_target_cm = current_alt_cm + dest.alt;
        }
    }

    // sanity check target
    int32_t alt_target_min_cm = current_alt_cm + (fms.ap.land_complete ? 100 : 0);
    alt_target_cm = MAX(alt_target_cm, alt_target_min_cm);

    // initialise yaw
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);

    // clear i term when we're taking off
    posctrl_init_z_controller(mode->pos_control);

    // initialise alt for WP_NAVALT_MIN and set completion alt
    mode_auto_takeoff_start(mode, alt_target_cm, alt_target_terrain);

    // set submode
    mauto_set_submode(mode_auto, AUTO_SUB_TAKEOFF);
}

// auto_loiter_start - initialises loitering in auto mode
//  returns success/failure because this can be called by exit_mission
bool mauto_loiter_start(ModeAuto* mode_auto)
{
    mode_base_t mode = &mode_auto->mode;

    // return failure if GPS is bad
    if (!fms_position_ok()) {
        return false;
    }
    mode_auto->auto_mode = AUTO_SUB_LOITER;

    // calculate stopping point
    Vector3f_t stopping_point;
    wpnav_get_wp_stopping_point(mode->wp_nav, &stopping_point);

    // initialise waypoint controller target to stopping point
    wpnav_set_wp_destination(mode->wp_nav, &stopping_point, false);

    // hold yaw at current heading
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);

    return true;
}

// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void mauto_wp_start(ModeAuto* mode_auto, const Location* dest_loc)
{
    mode_base_t mode = &mode_auto->mode;

    // init wpnav and set origin if transitioning from takeoff
    if (!wpnav_is_active(mode->wp_nav)) {
        Vector3f_t stopping_point;
        if (mode_auto->auto_mode == AUTO_SUB_TAKEOFF) {
            Vector3p takeoff_complete_pos;
            if (mode_auto_takeoff_get_position(&takeoff_complete_pos)) {
                stopping_point.x = takeoff_complete_pos.x;
                stopping_point.y = takeoff_complete_pos.y;
                stopping_point.z = takeoff_complete_pos.z;
            }
        }
        wpnav_wp_and_spline_init(mode->wp_nav, 0, stopping_point);
    }

    // send target to waypoint controller
    if (!wpnav_set_wp_destination_loc(mode->wp_nav, dest_loc)) {
        // failure to set destination can only be because of missing terrain data
        fms_failsafe_terrain_on_event();
        return;
    }

    // initialise yaw
    // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
    if (autoyaw_mode(mode->auto_yaw) != AUTO_YAW_ROI) {
        autoyaw_set_mode_to_default(mode->auto_yaw, false);
    }

    // set submode
    mauto_set_submode(mode_auto, AUTO_SUB_WP);
}

// auto_land_start - initialises controller to implement a landing
void mauto_land_start(ModeAuto* mode_auto)
{
    mode_base_t mode = &mode_auto->mode;

    // set horizontal speed and acceleration limits
    posctrl_set_max_speed_accel_xy(mode->pos_control, wpnav_get_default_speed_xy(mode->wp_nav), wpnav_get_wp_acceleration(mode->wp_nav));
    posctrl_set_correction_speed_accel_xy(mode->pos_control, wpnav_get_default_speed_xy(mode->wp_nav), wpnav_get_wp_acceleration(mode->wp_nav));

    // initialise the vertical position controller
    if (!posctrl_is_active_xy(mode->pos_control)) {
        posctrl_init_xy_controller(mode->pos_control);
    }

    // set vertical speed and acceleration limits
    posctrl_set_max_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));
    posctrl_set_correction_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));

    // initialise the vertical position controller
    if (!posctrl_is_active_z(mode->pos_control)) {
        posctrl_init_z_controller(mode->pos_control);
    }

    // initialise yaw
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);

#if 0
#if LANDING_GEAR_ENABLED == ENABLED
    // optionally deploy landing gear
    copter.landinggear.deploy_for_landing();
#endif

#if AP_FENCE_ENABLED
    // disable the fence on landing
    copter.fence.auto_disable_fence_for_landing();
#endif
#endif

    // reset flag indicating if pilot has applied roll or pitch inputs during landing
    fms.ap.land_repo_active = false;

    // this will be set true if prec land is later active
    fms.ap.prec_land_active = false;

    // set submode
    mauto_set_submode(mode_auto, AUTO_SUB_LAND);
}

// auto_rtl_start - initialises RTL in AUTO flight mode
void mauto_rtl_start(ModeAuto* mode_auto)
{
    // call regular rtl flight mode initialisation and ask it to ignore checks
    if (mode_init((mode_base_t)&fms.mode_rtl, true)) {
        mauto_set_submode(mode_auto, AUTO_SUB_RTL);
    } else {
        // this should never happen because RTL never fails init if argument is true
        //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
    }
}

#if NAV_GUIDED == ENABLED
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
void mauto_nav_guided_start(ModeAuto* mode_auto)
{
    // call regular guided flight mode initialisation
    if (!mode_init((mode_base_t)&fms.mode_guided, true)) {
        // this should never happen because guided mode never fails to init
        //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        return;
    }

    // initialise guided start time and position as reference for limit checking
    mguided_limit_init_time_and_pos();

    // set submode
    mauto_set_submode(mode_auto, AUTO_SUB_NAVGUIDED);
}
#endif //NAV_GUIDED

/********************************************************************************/
// Private function
/********************************************************************************/

// auto_init - initialise auto controller
static bool mauto_init(mode_base_t mode, bool ignore_checks)
{
    ((ModeAuto*)mode)->auto_RTL = false;
    if (mission_num_commands() > 1 || ignore_checks) {
        // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
        if (fms.motors->_armed && fms.ap.land_complete && !mission_starts_with_takeoff_cmd()) {
            gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
            return false;
        }

        ((ModeAuto*)mode)->auto_mode = AUTO_SUB_LOITER;

        // stop ROI from carrying over from previous runs of the mission
        // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
        if (autoyaw_mode(mode->auto_yaw) == AUTO_YAW_ROI) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }

        // initialise waypoint and spline controller
        wpnav_wp_and_spline_init2(mode->wp_nav);

        // set flag to start mission
        ((ModeAuto*)mode)->waiting_to_start = true;

        // initialise mission change check (ignore results)
        MissionChangeDetector_check_for_mission_change();

        // clear guided limits
        mguided_limit_clear();

        // reset flag indicating if pilot has applied roll or pitch inputs during landing
        fms.ap.land_repo_active = false;

        return true;
    } else {
        return false;
    }
}

// stop mission when we leave auto mode
static void mauto_exit(mode_base_t mode)
{
    if (mission_get_singleton()->_flags.state == MISSION_RUNNING) {
        mission_stop();
    }

    ((ModeAuto*)mode)->auto_RTL = false;
}

// auto_run - runs the auto controller
//      should be called at 100hz or more
static void mauto_run(mode_base_t mode)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;
    mb_mission_t mission = mission_get_singleton();

    // start or update mission
    if (mode_auto->waiting_to_start) {
        // don't start the mission until we have an origin
        Location loc;
        if (ahrs_get_origin(mode->ahrs, &loc)) {
            // start/resume the mission (based on MIS_RESTART parameter)
            mission_start_or_resume();
            mode_auto->waiting_to_start = false;

            // initialise mission change check (ignore results)
            MissionChangeDetector_check_for_mission_change();
        }
    } else {
        // check for mission changes
        if (MissionChangeDetector_check_for_mission_change()) {
            // if mission is running restart the current command if it is a waypoint or spline command
            if ((mission->_flags.state == MISSION_RUNNING) && (mode_auto->auto_mode == AUTO_SUB_WP)) {
                if (mission_restart_current_nav_cmd()) {
                    gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command");
                } else {
                    // failed to restart mission for some reason
                    gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed but failed to restart command");
                }
            }
        }

        mission_update();
    }

    // call the correct auto controller
    switch (mode_auto->auto_mode) {

    case AUTO_SUB_TAKEOFF:
        mauto_takeoff_run(mode);
        break;

    case AUTO_SUB_WP:
    case AUTO_SUB_CIRCLE_MOVE_TO_EDGE:
        mauto_wp_run(mode);
        break;

    case AUTO_SUB_LAND:
        mauto_land_run(mode);
        break;

    case AUTO_SUB_RTL:
        mauto_rtl_run(mode);
        break;

    case AUTO_SUB_CIRCLE:
        //circle_run();
        break;

    case AUTO_SUB_NAVGUIDED:
    case AUTO_SUB_NAV_SCRIPT_TIME:
#if NAV_GUIDED == ENABLED
        mauto_nav_guided_run(mode);
#endif
        break;

    case AUTO_SUB_LOITER:
        mauto_loiter_run(mode);
        break;

    case AUTO_SUB_LOITER_TO_ALT:
        mauto_loiter_to_alt_run(mode);
        break;

    case AUTO_SUB_NAV_PAYLOAD_PLACE:
        //payload_place_run();
        break;

    case AUTO_SUB_NAV_ATTITUDE_TIME:
        mauto_nav_attitude_time_run(mode);
        break;
    }

    // only pretend to be in auto RTL so long as mission still thinks its in a landing sequence or the mission has completed
    if (mode_auto->auto_RTL && (!(mission->_flags.in_landing_sequence || mission->_flags.state == MISSION_COMPLETE))) {
        mode_auto->auto_RTL = false;
        // log exit from Auto RTL
        //copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), ModeReason::AUTO_RTL_EXIT);
    }
}

static ModeNumber mauto_mode_number(mode_base_t mode) { return ((ModeAuto*)mode)->auto_RTL? AUTO_RTL : AUTO; }
static bool mauto_requires_GPS(mode_base_t mode) { return ((ModeAuto*)mode)->auto_mode != AUTO_SUB_NAV_ATTITUDE_TIME; }
static bool mauto_has_manual_throttle(mode_base_t mode) { return false; }
static bool mauto_allows_arming(mode_base_t mode, enum ArmingMethod method) { return ((fms.g.auto_options & (uint32_t)AutoAllowArming) != 0) && !((ModeAuto*)mode)->auto_RTL; }
static bool mauto_is_autopilot(mode_base_t mode) { return true; }
static bool mauto_in_guided_mode(mode_base_t mode) { return ((ModeAuto*)mode)->auto_mode == AUTO_SUB_NAVGUIDED || ((ModeAuto*)mode)->auto_mode == AUTO_SUB_NAV_SCRIPT_TIME; }
static bool mauto_requires_terrain_failsafe(mode_base_t mode) { return true; }
static bool mauto_has_user_takeoff(mode_base_t mode, bool must_navigate) { return false;}

static const char *mauto_name(mode_base_t mode) { return ((ModeAuto*)mode)->auto_RTL? "AUTO RTL" : "AUTO"; }
static const char *mauto_name4(mode_base_t mode) { return ((ModeAuto*)mode)->auto_RTL? "ARTL" : "AUTO"; }

static uint32_t mauto_wp_distance(mode_base_t mode)
{
    switch (((ModeAuto*)mode)->auto_mode) {
    case AUTO_SUB_CIRCLE:
        return 0;
    case AUTO_SUB_WP:
    case AUTO_SUB_CIRCLE_MOVE_TO_EDGE:
    default:
        return wpnav_get_wp_distance_to_destination(mode->wp_nav);
    }
}

static int32_t mauto_wp_bearing(mode_base_t mode)
{
    switch (((ModeAuto*)mode)->auto_mode) {
    case AUTO_SUB_CIRCLE:
        return 0;
    case AUTO_SUB_WP:
    case AUTO_SUB_CIRCLE_MOVE_TO_EDGE:
    default:
        return wpnav_get_wp_bearing_to_destination(mode->wp_nav);
    }
}

static float mauto_crosstrack_error(mode_base_t mode)
{
    return wpnav_crosstrack_error(mode->wp_nav);

}

static bool mauto_get_wp(mode_base_t mode, Location *loc)
{
    switch (((ModeAuto*)mode)->auto_mode) {
    case AUTO_SUB_NAVGUIDED:
        return mode_get_wp((mode_base_t)&fms.mode_guided, loc);
    case AUTO_SUB_WP:
        return wpnav_get_oa_wp_destination(mode->wp_nav, loc);
    case AUTO_SUB_RTL:
        return mode_get_wp((mode_base_t)&fms.mode_rtl, loc);
    default:
        return false;
    }
}

// pause - Prevent aircraft from progressing along the track
static bool mauto_pause(mode_base_t mode)
{
    // do not pause if already paused or not in the WP sub mode or already reached to the destination
    if(wpnav_paused(mode->wp_nav) || ((ModeAuto*)mode)->auto_mode != AUTO_SUB_WP || wpnav_reached_wp_destination(mode->wp_nav)) {
        return false;
    }

    wpnav_set_pause(mode->wp_nav);
    return true;
}

// resume - Allow aircraft to progress along the track
static bool mauto_resume(mode_base_t mode)
{
    // do not resume if not paused before
    if(!wpnav_paused(mode->wp_nav)) {
        return false;
    }

    wpnav_set_resume(mode->wp_nav);
    return true;
}

static bool mauto_is_landing(mode_base_t mode)
{
    switch(((ModeAuto*)mode)->auto_mode) {
    case AUTO_SUB_LAND:
        return true;
    case AUTO_SUB_RTL:
        return mode_is_landing((mode_base_t)&fms.mode_rtl);
    default:
        return false;
    }
    return false;
}

static bool mauto_is_taking_off(mode_base_t mode)
{
    return ((((ModeAuto*)mode)->auto_mode == AUTO_SUB_TAKEOFF) && !(*mode->auto_takeoff_complete));
}

// returns true if pilot's yaw input should be used to adjust vehicle's heading
static bool mauto_use_pilot_yaw(mode_base_t mode)
{
    return (fms.g.auto_options & (uint32_t)(AutoIgnorePilotYaw)) == 0;
}

static bool mauto_set_speed_xy(mode_base_t mode, float speed_xy_cms)
{
    wpnav_set_speed_xy(mode->wp_nav, speed_xy_cms);
    return true;
}

static bool mauto_set_speed_up(mode_base_t mode, float speed_up_cms)
{
    wpnav_set_speed_up(mode->wp_nav, speed_up_cms);
    return true;
}

static bool mauto_set_speed_down(mode_base_t mode, float speed_down_cms)
{
    wpnav_set_speed_down(mode->wp_nav, speed_down_cms);
    return true;
}

static bool mauto_start_command(const mission_command_t cmd)
{
#if 0
    // To-Do: logging when new commands start/end
    if (copter.should_log(MASK_LOG_CMD)) {
        copter.logger.Write_Mission_Cmd(mission, cmd);
    }
#endif

    switch(cmd->id) {

    ///
    /// navigation commands
    ///
    case MAV_CMD_NAV_VTOL_TAKEOFF:
    case MAV_CMD_NAV_TAKEOFF:                   // 22
        mauto_do_takeoff(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_NAV_WAYPOINT:                  // 16  Navigate to Waypoint
        mauto_do_nav_wp(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_NAV_VTOL_LAND:
    case MAV_CMD_NAV_LAND:              // 21 LAND to Waypoint
        mauto_do_land(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_NAV_LOITER_UNLIM:              // 17 Loiter indefinitely
        mauto_do_loiter_unlimited(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_NAV_LOITER_TURNS:              //18 Loiter N Times
        //do_circle(cmd);
        break;

    case MAV_CMD_NAV_LOITER_TIME:              // 19
        mauto_do_loiter_time(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_NAV_LOITER_TO_ALT:
        mauto_do_loiter_to_alt(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_NAV_RETURN_TO_LAUNCH:             //20
        mauto_do_RTL(&fms.mode_auto);
        break;

    case MAV_CMD_NAV_SPLINE_WAYPOINT:           // 82  Navigate to Waypoint using spline
        mauto_do_spline_wp(&fms.mode_auto, cmd);
        break;

#if NAV_GUIDED == ENABLED
    case MAV_CMD_NAV_GUIDED_ENABLE:             // 92  accept navigation commands from external nav computer
        mauto_do_nav_guided_enable(&fms.mode_auto, cmd);
        break;
#endif

    case MAV_CMD_NAV_DELAY:                    // 93 Delay the next navigation command
        mauto_do_nav_delay(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_NAV_PAYLOAD_PLACE:              // 94 place at Waypoint
        //do_payload_place(cmd);
        break;

    case MAV_CMD_NAV_ATTITUDE_TIME:
        mauto_do_nav_attitude_time(&fms.mode_auto, cmd);
        break;

    //
    // conditional commands
    //
    case MAV_CMD_CONDITION_DELAY:             // 112
        mauto_do_wait_delay(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_CONDITION_DISTANCE:             // 114
        mauto_do_within_distance(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_CONDITION_YAW:             // 115
        mauto_do_yaw(&fms.mode_auto, cmd);
        break;

    ///
    /// do commands
    ///
    case MAV_CMD_DO_CHANGE_SPEED:             // 178
        mauto_do_change_speed(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_DO_SET_HOME:             // 179
        mauto_do_set_home(&fms.mode_auto, cmd);
        break;

    case MAV_CMD_DO_SET_ROI:                // 201
        // point the copter and camera at a region of interest (ROI)
        mauto_do_roi(&fms.mode_auto, cmd);
        break;

#if 0
    case MAV_CMD_DO_MOUNT_CONTROL:          // 205
        // point the camera to a specified angle
        do_mount_control(cmd);
        break;

    case MAV_CMD_DO_FENCE_ENABLE:
#if AP_FENCE_ENABLED
        if (cmd.p1 == 0) { //disable
            copter.fence.enable(false);
            gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled");
        } else { //enable fence
            copter.fence.enable(true);
            gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled");
        }
#endif //AP_FENCE_ENABLED
        break;
#endif

#if NAV_GUIDED == ENABLED
    case MAV_CMD_DO_GUIDED_LIMITS:                      // 220  accept guided mode limits
        mauto_do_guided_limits(&fms.mode_auto, cmd);
        break;
#endif

#if 0
#if WINCH_ENABLED == ENABLED
    case MAV_CMD_DO_WINCH:                             // Mission command to control winch
        //do_winch(cmd);
        break;
#endif
#endif

    case MAV_CMD_DO_LAND_START:
        break;

    default:
        // unable to use the command, allow the vehicle to try the next command
        return false;
    }

    // always return success
    return true;
}

static bool mauto_verify_command(const mission_command_t cmd)
{
    if (fms.flightmode != (mode_base_t)&fms.mode_auto) {
        return false;
    }

    bool cmd_complete = false;

    switch (cmd->id) {
    //
    // navigation commands
    //
    case MAV_CMD_NAV_VTOL_TAKEOFF:
    case MAV_CMD_NAV_TAKEOFF:
        cmd_complete = mauto_verify_takeoff(fms.flightmode);
        break;

    case MAV_CMD_NAV_WAYPOINT:
        cmd_complete = mauto_verify_nav_wp(fms.flightmode, cmd);
        break;

    case MAV_CMD_NAV_VTOL_LAND:
    case MAV_CMD_NAV_LAND:
        cmd_complete = mauto_verify_land(fms.flightmode);
        break;

    case MAV_CMD_NAV_PAYLOAD_PLACE:
        //cmd_complete = verify_payload_place();
        break;

    case MAV_CMD_NAV_LOITER_UNLIM:
        cmd_complete = mauto_verify_loiter_unlimited(fms.flightmode);
        break;

    case MAV_CMD_NAV_LOITER_TURNS:
        //cmd_complete = verify_circle(cmd);
        break;

    case MAV_CMD_NAV_LOITER_TIME:
        cmd_complete = mauto_verify_loiter_time(fms.flightmode, cmd);
        break;

    case MAV_CMD_NAV_LOITER_TO_ALT:
        return mauto_verify_loiter_to_alt(fms.flightmode);

    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
        cmd_complete = mauto_verify_RTL(fms.flightmode);
        break;

    case MAV_CMD_NAV_SPLINE_WAYPOINT:
        cmd_complete = mauto_verify_spline_wp(fms.flightmode, cmd);
        break;

#if NAV_GUIDED == ENABLED
    case MAV_CMD_NAV_GUIDED_ENABLE:
        cmd_complete = mauto_verify_nav_guided_enable(fms.flightmode, cmd);
        break;
#endif

     case MAV_CMD_NAV_DELAY:
        cmd_complete = mauto_verify_nav_delay(fms.flightmode, cmd);
        break;

    case MAV_CMD_NAV_ATTITUDE_TIME:
        cmd_complete = mauto_verify_nav_attitude_time(fms.flightmode, cmd);
        break;

    ///
    /// conditional commands
    ///
    case MAV_CMD_CONDITION_DELAY:
        cmd_complete = mauto_verify_wait_delay(fms.flightmode);
        break;

    case MAV_CMD_CONDITION_DISTANCE:
        cmd_complete = mauto_verify_within_distance(fms.flightmode);
        break;

    case MAV_CMD_CONDITION_YAW:
        cmd_complete = mauto_verify_yaw(fms.flightmode);
        break;

    // do commands (always return true)
    case MAV_CMD_DO_CHANGE_SPEED:
    case MAV_CMD_DO_SET_HOME:
    case MAV_CMD_DO_SET_ROI:
    case MAV_CMD_DO_MOUNT_CONTROL:
    case MAV_CMD_DO_GUIDED_LIMITS:
    case MAV_CMD_DO_FENCE_ENABLE:
    case MAV_CMD_DO_WINCH:
    case MAV_CMD_DO_LAND_START:
        cmd_complete = true;
        break;

    default:
        // error message
        gcs_send_text(MAV_SEVERITY_WARNING,"Skipping invalid cmd #%i",cmd->id);
        // return true if we do not recognize the command so that we move on to the next command
        cmd_complete = true;
        break;
    }


    // send message to GCS
    if (cmd_complete) {
        fms_mission_item_reached_send(cmd->index);
    }

    return cmd_complete;
}

static void mauto_exit_mission()
{
    // play a tone
    notify_events.mission_complete = 1;

    // if we are not on the ground switch to loiter or land
    if (!fms.ap.land_complete) {
        // try to enter loiter but if that fails land
        if (!mauto_loiter_start(&fms.mode_auto)) {
            fms_set_mode(LAND, MODE_REASON_MISSION_END);
        }
    } else {
        // if we've landed it's safe to disarm
        mb_arming_disarm(ARMING_METHOD_MISSIONEXIT, true);
    }
}

// takeoff_run - takeoff in auto mode
//      called by auto_run at 100hz or more
static void mauto_takeoff_run(mode_base_t mode)
{
    // if the user doesn't want to raise the throttle we can set it automatically
    // note that this can defeat the disarm check on takeoff
    if ((fms.g.auto_options & (int32_t)AutoAllowTakeOffWithoutRaisingThrottle) != 0) {
        fms_set_auto_armed(true);
    }
    mode_auto_takeoff_run(mode);
}

// auto_loiter_run - loiter in AUTO flight mode
//      called by auto_run at 100hz or more
static void mauto_loiter_run(mode_base_t mode)
{
    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // accept pilot input of yaw
    float target_yaw_rate = 0;
    if (!fms.failsafe.radio && mode_use_pilot_yaw(mode)) {
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(rc_norm_input_dz(mode->channel_yaw));
    }

    // set motors to full range
    Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // run waypoint and z-axis position controller
    fms_failsafe_terrain_set_status(wpnav_update_wpnav(mode->wp_nav));

    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t thrust_vector = wpnav_get_thrust_vector(mode->wp_nav);
    attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate, true);
}

// auto_wp_run - runs the auto waypoint controller
//      called by auto_run at 100hz or more
static void mauto_wp_run(mode_base_t mode)
{
    // process pilot's yaw input
    float target_yaw_rate = 0;
    if (!fms.failsafe.radio && mode_use_pilot_yaw(mode)) {
        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(rc_norm_input_dz(mode->channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // run waypoint controller
    fms_failsafe_terrain_set_status(wpnav_update_wpnav(mode->wp_nav));

    // WP_Nav has set the vertical position control targets
    // run the vertical position controller and set output throttle
    posctrl_update_z_controller(mode->pos_control);

    Vector3f_t thrust_vector = wpnav_get_thrust_vector(mode->wp_nav);

    // call attitude controller
    if (autoyaw_mode(mode->auto_yaw) == AUTO_YAW_HOLD) {
        // roll & pitch from waypoint controller, yaw rate from pilot
        attctrl_input_thrust_vector_rate_heading(mode->attitude_control, &thrust_vector, target_yaw_rate, true);
    } else {
        // roll, pitch from waypoint controller, yaw heading from auto_heading()
        attctrl_input_thrust_vector_heading(mode->attitude_control, &thrust_vector, autoyaw_yaw(mode->auto_yaw), autoyaw_rate_cds(mode->auto_yaw));
    }
}

// auto_land_run - lands in auto mode
//      called by auto_run at 100hz or more
static void mauto_land_run(mode_base_t mode)
{

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // set motors to full range
    Motors_set_desired_spool_state(mode->motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // run normal landing or precision landing (if enabled)
    mode_land_run_normal_or_precland(mode, false);
}

// auto_rtl_run - rtl in AUTO flight mode
//      called by auto_run at 100hz or more
static void mauto_rtl_run(mode_base_t mode)
{
    // call regular rtl flight mode run function
    mrtl_run2((mode_base_t)&fms.mode_rtl, false);
}

// auto_loiter_run - loiter to altitude in AUTO flight mode
//      called by auto_run at 100hz or more
static void mauto_loiter_to_alt_run(mode_base_t mode)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed() || !Motors_get_interlock(mode->motors)) {
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // possibly just run the waypoint controller:
    if (!mode_auto->loiter_to_alt.reached_destination_xy) {
        mode_auto->loiter_to_alt.reached_destination_xy = wpnav_reached_wp_destination_xy(mode->wp_nav);
        if (!mode_auto->loiter_to_alt.reached_destination_xy) {
            mauto_wp_run(mode);
            return;
        }
    }

    if (!mode_auto->loiter_to_alt.loiter_start_done) {
        // set horizontal speed and acceleration limits
        posctrl_set_max_speed_accel_xy(mode->pos_control, wpnav_get_default_speed_xy(mode->wp_nav), wpnav_get_wp_acceleration(mode->wp_nav));
        posctrl_set_correction_speed_accel_xy(mode->pos_control, wpnav_get_default_speed_xy(mode->wp_nav), wpnav_get_wp_acceleration(mode->wp_nav));

        if (!posctrl_is_active_xy(mode->pos_control)) {
            posctrl_init_xy_controller(mode->pos_control);
        }

        mode_auto->loiter_to_alt.loiter_start_done = true;
    }
    const float alt_error_cm = fms.current_loc.alt - mode_auto->loiter_to_alt.alt;
    if (fabsf(alt_error_cm) < 5.0) { // random numbers R US
        mode_auto->loiter_to_alt.reached_alt = true;
    } else if (alt_error_cm * mode_auto->loiter_to_alt.alt_error_cm < 0) {
        // we were above and are now below, or vice-versa
        mode_auto->loiter_to_alt.reached_alt = true;
    }
    mode_auto->loiter_to_alt.alt_error_cm = alt_error_cm;

    // loiter...

    mode_land_run_horizontal_control(mode);

    // Compute a vertical velocity demand such that the vehicle
    // approaches the desired altitude.
    float target_climb_rate = control_sqrt_controller(
        -alt_error_cm,
        posctrl_get_pos_z_p(mode->pos_control)->_kp,
        posctrl_get_max_accel_z_cmss(mode->pos_control),
        fms.G_Dt);
    target_climb_rate = math_constrain_float(target_climb_rate, posctrl_get_max_speed_down_cms(mode->pos_control), posctrl_get_max_speed_up_cms(mode->pos_control));

    // get avoidance adjusted climb rate
    //target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);

    // update the vertical offset based on the surface measurement
    //copter.surface_tracking.update_surface_offset();

    // Send the commanded climb rate to the position controller
    posctrl_set_pos_target_z_from_climb_rate_cm(mode->pos_control, target_climb_rate);

    posctrl_update_z_controller(mode->pos_control);
}

#if NAV_GUIDED == ENABLED
// auto_nav_guided_run - allows control by external navigation controller
//      called by auto_run at 100hz or more
static void mauto_nav_guided_run(mode_base_t mode)
{
    // call regular guided flight mode run function
    mode_run((mode_base_t)&fms.mode_guided);
}
#endif  // NAV_GUIDED

// maintain an attitude for a specified time
static void mauto_nav_attitude_time_run(mode_base_t mode)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed() || !Motors_get_interlock(mode->motors)) {
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // constrain climb rate
    float target_climb_rate_cms = math_constrain_float(mode_auto->nav_attitude_time.climb_rate * 100.0, posctrl_get_max_speed_down_cms(mode->pos_control), posctrl_get_max_speed_up_cms(mode->pos_control));

    // get avoidance adjusted climb rate
    //target_climb_rate_cms = get_avoidance_adjusted_climbrate(target_climb_rate_cms);

    // limit and scale lean angles
    const float angle_limit_cd = MAX(1000.0f, MIN(fms.g.angle_max, attctrl_get_althold_lean_angle_max_cd(mode->attitude_control)));
    Vector2f_t target_rp_cd = {mode_auto->nav_attitude_time.roll_deg * 100.0f, mode_auto->nav_attitude_time.pitch_deg * 100.0f};
    vec2_limit_length(&target_rp_cd , angle_limit_cd);

    // send targets to attitude controller
    attctrl_input_euler_angle_roll_pitch_yaw(mode->attitude_control, target_rp_cd.x, target_rp_cd.y, mode_auto->nav_attitude_time.yaw_deg * 100, true);

    // Send the commanded climb rate to the position controller
    posctrl_set_pos_target_z_from_climb_rate_cm(mode->pos_control, target_climb_rate_cms);

    posctrl_update_z_controller(mode->pos_control);
}

// sets the target_loc's alt to the vehicle's current alt but does not change target_loc's frame
// in the case of terrain altitudes either the terrain database or the rangefinder may be used
// returns true on success, false on failure
static bool mauto_shift_alt_to_current_alt(mode_base_t mode, Location* target_loc)
{
    // if terrain alt using rangefinder is being used then set alt to current rangefinder altitude
    if ((location_get_alt_frame(target_loc) == ALT_FRAME_ABOVE_TERRAIN) &&
        (wpnav_get_terrain_source(mode->wp_nav) == WPNAV_TERRAIN_FROM_RANGEFINDER)) {
        int32_t curr_rngfnd_alt_cm;
        if (fms_get_rangefinder_height_interpolated_cm(&curr_rngfnd_alt_cm)) {
            // wp_nav is using rangefinder so use current rangefinder alt
            location_set_alt_cm(target_loc, MAX(curr_rngfnd_alt_cm, 200), ALT_FRAME_ABOVE_TERRAIN);
            return true;
        }
        return false;
    }

    // take copy of current location and change frame to match target
    Location currloc = fms.current_loc;
    if (!location_change_alt_frame(&currloc, location_get_alt_frame(target_loc))) {
        // this could fail due missing terrain database alt
        return false;
    }

    // set target_loc's alt
    location_set_alt_cm(target_loc, currloc.alt, location_get_alt_frame(&currloc));
    return true;
}

/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
// do_takeoff - initiate takeoff navigation command
static void mauto_do_takeoff(ModeAuto* mode_auto, const mission_command_t cmd)
{
    // Set wp navigation target to safe altitude above current position
    mauto_takeoff_start(mode_auto, &cmd->content.location);
}

static Location mauto_loc_from_cmd(const mission_command_t cmd, const Location* default_loc)
{
    Location ret = cmd->content.location;

    // use current lat, lon if zero
    if (ret.lat == 0 && ret.lng == 0) {
        ret.lat = default_loc->lat;
        ret.lng = default_loc->lng;
    }
    // use default altitude if not provided in cmd
    if (ret.alt == 0) {
        // set to default_loc's altitude but in command's alt frame
        // note that this may use the terrain database
        int32_t default_alt;
        if (location_get_alt_cm(default_loc, location_get_alt_frame(&ret), &default_alt)) {
            location_set_alt_cm(&ret, default_alt, location_get_alt_frame(&ret));
        } else {
            // default to default_loc's altitude and frame
            location_set_alt_cm(&ret, default_loc->alt, location_get_alt_frame(default_loc));
        }
    }
    return ret;
}

// do_nav_wp - initiate move to next waypoint
static void mauto_do_nav_wp(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mode_base_t mode = &mode_auto->mode;

    // calculate default location used when lat, lon or alt is zero
    Location default_loc = fms.current_loc;
    if (wpnav_is_active(mode->wp_nav) && wpnav_reached_wp_destination(mode->wp_nav)) {
        if (!wpnav_get_wp_destination_loc(mode->wp_nav, &default_loc)) {
            // this should never happen
            //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        }
    }

    // init wpnav and set origin if transitioning from takeoff
    if (!wpnav_is_active(mode->wp_nav)) {
        Vector3f_t stopping_point;
        if (mode_auto->auto_mode == AUTO_SUB_TAKEOFF) {
            Vector3p takeoff_complete_pos;
            if (mode_auto_takeoff_get_position(&takeoff_complete_pos)) {
                stopping_point.x = takeoff_complete_pos.x;
                stopping_point.y = takeoff_complete_pos.y;
                stopping_point.z = takeoff_complete_pos.z;
            }
        }
        wpnav_wp_and_spline_init(mode->wp_nav, 0, stopping_point);
    }

    // get waypoint's location from command and send to wp_nav
    const Location dest_loc = mauto_loc_from_cmd(cmd, &default_loc);
    if (!wpnav_set_wp_destination_loc(mode->wp_nav, &dest_loc)) {
        // failure to set destination can only be because of missing terrain data
        fms_failsafe_terrain_on_event();
        return;
    }

    // this will be used to remember the time in millis after we reach or pass the WP.
    mode_auto->loiter_time = 0;
    // this is the delay, stored in seconds
    mode_auto->loiter_time_max = cmd->p1;

    // set next destination if necessary
    if (!mauto_set_next_wp(mode, cmd, &dest_loc)) {
        // failure to set next destination can only be because of missing terrain data
        fms_failsafe_terrain_on_event();
        return;
    }

    // initialise yaw
    // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
    if (autoyaw_mode(mode->auto_yaw) != AUTO_YAW_ROI) {
        autoyaw_set_mode_to_default(mode->auto_yaw, false);
    }

    // set submode
    mauto_set_submode(mode_auto, AUTO_SUB_WP);
}

// checks the next mission command and adds it as a destination if necessary
// supports both straight line and spline waypoints
// cmd should be the current command
// default_loc should be the destination from the current_cmd but corrected for cases where user set lat, lon or alt to zero
// returns true on success, false on failure which should only happen due to a failure to retrieve terrain data
static bool mauto_set_next_wp(mode_base_t mode, const mission_command_t current_cmd, const Location *default_loc)
{
    // do not add next wp if current command has a delay meaning the vehicle will stop at the destination
    if (current_cmd->p1 > 0) {
        return true;
    }

    // do not add next wp if there are no more navigation commands
    struct Mission_Command next_cmd;
    if (!mission_get_next_nav_cmd(current_cmd->index+1, &next_cmd)) {
        return true;
    }

    // whether vehicle should stop at the target position depends upon the next command
    switch (next_cmd.id) {
    case MAV_CMD_NAV_WAYPOINT:
    case MAV_CMD_NAV_LOITER_UNLIM:
    case MAV_CMD_NAV_LOITER_TIME: {
        const Location dest_loc = mauto_loc_from_cmd(current_cmd, default_loc);
        const Location next_dest_loc = mauto_loc_from_cmd(&next_cmd, &dest_loc);
        return wpnav_set_wp_destination_next_loc(mode->wp_nav, &next_dest_loc);
    }
    case MAV_CMD_NAV_SPLINE_WAYPOINT: {
        // get spline's location and next location from command and send to wp_nav
        Location next_dest_loc, next_next_dest_loc;
        bool next_next_dest_loc_is_spline;
        mauto_get_spline_from_cmd(&next_cmd, default_loc, &next_dest_loc, &next_next_dest_loc, &next_next_dest_loc_is_spline);
        return wpnav_set_spline_destination_next_loc(mode->wp_nav, &next_dest_loc, &next_next_dest_loc, next_next_dest_loc_is_spline);
    }
    case MAV_CMD_NAV_VTOL_LAND:
    case MAV_CMD_NAV_LAND:
        // stop because we may change between rel,abs and terrain alt types
    case MAV_CMD_NAV_LOITER_TURNS:
    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
    case MAV_CMD_NAV_VTOL_TAKEOFF:
    case MAV_CMD_NAV_TAKEOFF:
        // always stop for RTL and takeoff commands
    default:
        // for unsupported commands it is safer to stop
        break;
    }

    return true;
}

// calculate locations required to build a spline curve from a mission command
// dest_loc is populated from cmd's location using default_loc in cases where the lat and lon or altitude is zero
// next_dest_loc and nest_dest_loc_is_spline is filled in with the following navigation command's location if it exists.  If it does not exist it is set to the dest_loc and false
static void mauto_get_spline_from_cmd(const mission_command_t cmd, const Location* default_loc, Location* dest_loc, Location* next_dest_loc, bool* next_dest_loc_is_spline)
{
    *dest_loc = mauto_loc_from_cmd(cmd, default_loc);

    // if there is no delay at the end of this segment get next nav command
    struct Mission_Command temp_cmd;
    if (cmd->p1 == 0 && mission_get_next_nav_cmd(cmd->index+1, &temp_cmd)) {
        *next_dest_loc = mauto_loc_from_cmd(&temp_cmd, dest_loc);
        *next_dest_loc_is_spline = temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT;
    } else {
        *next_dest_loc = (*dest_loc);
        *next_dest_loc_is_spline = false;
    }
}

// do_land - initiate landing procedure
static void mauto_do_land(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mode_base_t mode = &mode_auto->mode;

    // To-Do: check if we have already landed

    // if location provided we fly to that location at current altitude
    if (cmd->content.location.lat != 0 || cmd->content.location.lng != 0) {
        // set state to fly to location
        mode_auto->state = AutoFlyToLocation;

        // convert cmd to location class
        Location target_loc = cmd->content.location;
        if (!mauto_shift_alt_to_current_alt(mode, &target_loc)) {
            // this can only fail due to missing terrain database alt or rangefinder alt
            // use current alt-above-home and report error
            location_set_alt_cm(&target_loc, fms.current_loc.alt, ALT_FRAME_ABOVE_HOME);
            //AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
            gcs_send_text(MAV_SEVERITY_CRITICAL, "Land: no terrain data, using alt-above-home");
        }

        mauto_wp_start(mode_auto, &target_loc);
    } else {
        // set landing state
        mode_auto->state = AutoDescending;

        // initialise landing controller
        mauto_land_start(mode_auto);
    }
}

// do_RTL - start Return-to-Launch
static void mauto_do_RTL(ModeAuto* mode_auto)
{
    // start rtl in auto flight mode
    mauto_rtl_start(mode_auto);
}

// do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode
static void mauto_do_loiter_unlimited(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mode_base_t mode = &mode_auto->mode;

    // convert back to location
    Location target_loc = cmd->content.location;

    // use current location if not provided
    if (target_loc.lat == 0 && target_loc.lng == 0) {
        // To-Do: make this simpler
        Vector2f_t temp_pos;
        wpnav_get_wp_stopping_point_xy(mode->wp_nav, &temp_pos);
        Location temp_loc;
        Location_from_vector_xy_alt2(&temp_loc, &temp_pos, ALT_FRAME_ABOVE_ORIGIN);

        target_loc.lat = temp_loc.lat;
        target_loc.lng = temp_loc.lng;
    }

    // use current altitude if not provided
    // To-Do: use z-axis stopping point instead of current alt
    if (target_loc.alt == 0) {
        // set to current altitude but in command's alt frame
        int32_t curr_alt;
        if (location_get_alt_cm(&fms.current_loc, location_get_alt_frame(&target_loc), &curr_alt)) {
            location_set_alt_cm(&target_loc, curr_alt, location_get_alt_frame(&target_loc));
        } else {
            // default to current altitude as alt-above-home
            location_set_alt_cm(&target_loc, fms.current_loc.alt,
                                  location_get_alt_frame(&fms.current_loc));
        }
    }

    // start way point navigator and provide it the desired location
    mauto_wp_start(mode_auto, &target_loc);
}

// do_loiter_time - initiate loitering at a point for a given time period
// note: caller should set yaw_mode
static void mauto_do_loiter_time(ModeAuto* mode_auto, const mission_command_t cmd)
{
    // re-use loiter unlimited
    mauto_do_loiter_unlimited(mode_auto, cmd);

    // setup loiter timer
    mode_auto->loiter_time     = 0;
    mode_auto->loiter_time_max = cmd->p1;     // units are (seconds)
}

// do_loiter_alt - initiate loitering at a point until a given altitude is reached
// note: caller should set yaw_mode
static void mauto_do_loiter_to_alt(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mode_base_t mode = &mode_auto->mode;

    // re-use loiter unlimited
    mauto_do_loiter_unlimited(mode_auto, cmd);

    // if we aren't navigating to a location then we have to adjust
    // altitude for current location
    Location target_loc = cmd->content.location;
    if (target_loc.lat == 0 && target_loc.lng == 0) {
        target_loc.lat = mode->ahrs->curr_loc.lat;
        target_loc.lng = mode->ahrs->curr_loc.lng;
    }

    if (!location_get_alt_cm(&target_loc, ALT_FRAME_ABOVE_HOME, &mode_auto->loiter_to_alt.alt)) {
        mode_auto->loiter_to_alt.reached_destination_xy = true;
        mode_auto->loiter_to_alt.reached_alt = true;
        gcs_send_text(MAV_SEVERITY_INFO, "bad do_loiter_to_alt");
        return;
    }
    mode_auto->loiter_to_alt.reached_destination_xy = false;
    mode_auto->loiter_to_alt.loiter_start_done = false;
    mode_auto->loiter_to_alt.reached_alt = false;
    mode_auto->loiter_to_alt.alt_error_cm = 0;

    // set vertical speed and acceleration limits
    posctrl_set_max_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));
    posctrl_set_correction_speed_accel_z(mode->pos_control, wpnav_get_default_speed_down(mode->wp_nav), wpnav_get_default_speed_up(mode->wp_nav), wpnav_get_accel_z(mode->wp_nav));

    // set submode
    mauto_set_submode(mode_auto, AUTO_SUB_LOITER_TO_ALT);
}

// do_spline_wp - initiate move to next waypoint
static void mauto_do_spline_wp(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mode_base_t mode = &mode_auto->mode;

    // calculate default location used when lat, lon or alt is zero
    Location default_loc = fms.current_loc;
    if (wpnav_is_active(mode->wp_nav) && wpnav_reached_wp_destination(mode->wp_nav)) {
        if (!wpnav_get_wp_destination_loc(mode->wp_nav, &default_loc)) {
            // this should never happen
            //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        }
    }

    // get spline's location and next location from command and send to wp_nav
    Location dest_loc, next_dest_loc;
    bool next_dest_loc_is_spline;
    mauto_get_spline_from_cmd(cmd, &default_loc, &dest_loc, &next_dest_loc, &next_dest_loc_is_spline);
    if (!wpnav_set_spline_destination_loc(mode->wp_nav, &dest_loc, &next_dest_loc, next_dest_loc_is_spline)) {
        // failure to set destination can only be because of missing terrain data
        fms_failsafe_terrain_on_event();
        return;
    }

    // this will be used to remember the time in millis after we reach or pass the WP.
    mode_auto->loiter_time = 0;
    // this is the delay, stored in seconds
    mode_auto->loiter_time_max = cmd->p1;

    // set next destination if necessary
    if (!mauto_set_next_wp(mode, cmd, &dest_loc)) {
        // failure to set next destination can only be because of missing terrain data
        fms_failsafe_terrain_on_event();
        return;
    }

    // initialise yaw
    // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
    if (autoyaw_mode(mode->auto_yaw) != AUTO_YAW_ROI) {
        autoyaw_set_mode_to_default(mode->auto_yaw, false);
    }

    // set submode
    mauto_set_submode(mode_auto, AUTO_SUB_WP);
}

#if NAV_GUIDED == ENABLED
// do_nav_guided_enable - initiate accepting commands from external nav computer
static void mauto_do_nav_guided_enable(ModeAuto* mode_auto, const mission_command_t cmd)
{
    if (cmd->p1 > 0) {
        // start guided within auto
        mauto_nav_guided_start(mode_auto);
    }
}

// do_guided_limits - pass guided limits to guided controller
static void mauto_do_guided_limits(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mguided_limit_set(
        cmd->p1 * 1000, // convert seconds to ms
        cmd->content.guided_limits.alt_min * 100.0f,    // convert meters to cm
        cmd->content.guided_limits.alt_max * 100.0f,    // convert meters to cm
        cmd->content.guided_limits.horiz_max * 100.0f); // convert meters to cm
}
#endif  // NAV_GUIDED

// do_nav_delay - Delay the next navigation command
static void mauto_do_nav_delay(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mode_auto->nav_delay_time_start_ms = time_millis();

    if (cmd->content.nav_delay.seconds > 0) {
        // relative delay
        mode_auto->nav_delay_time_max_ms = cmd->content.nav_delay.seconds * 1000; // convert seconds to milliseconds
    } else {
        // absolute delay to utc time
        mode_auto->nav_delay_time_max_ms = rtc_get_time_utc(cmd->content.nav_delay.hour_utc, cmd->content.nav_delay.min_utc, cmd->content.nav_delay.sec_utc, 0);
    }
    gcs_send_text(MAV_SEVERITY_INFO, "Delaying %u sec", (unsigned)(mode_auto->nav_delay_time_max_ms/1000));
}

// start maintaining an attitude for a specified time
static void mauto_do_nav_attitude_time(ModeAuto* mode_auto, const mission_command_t cmd)
{
    // copy command arguments into local structure
    mode_auto->nav_attitude_time.roll_deg = cmd->content.nav_attitude_time.roll_deg;
    mode_auto->nav_attitude_time.pitch_deg = cmd->content.nav_attitude_time.pitch_deg;
    mode_auto->nav_attitude_time.yaw_deg = cmd->content.nav_attitude_time.yaw_deg;
    mode_auto->nav_attitude_time.climb_rate = cmd->content.nav_attitude_time.climb_rate;
    mode_auto->nav_attitude_time.start_ms = time_millis();

    mauto_set_submode(mode_auto, AUTO_SUB_NAV_ATTITUDE_TIME);
}

/********************************************************************************/
// Condition (May) commands
/********************************************************************************/

static void mauto_do_wait_delay(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mode_auto->condition_start = time_millis();
    mode_auto->condition_value = cmd->content.delay.seconds * 1000;     // convert seconds to milliseconds
}

static void mauto_do_within_distance(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mode_auto->condition_value  = cmd->content.distance.meters * 100;
}

static void mauto_do_yaw(ModeAuto* mode_auto, const mission_command_t cmd)
{
    autoyaw_set_fixed_yaw(mode_auto->mode.auto_yaw,
        cmd->content.yaw.angle_deg,
        cmd->content.yaw.turn_rate_dps,
        cmd->content.yaw.direction,
        cmd->content.yaw.relative_angle > 0);
}

/********************************************************************************/
// Do (Now) commands
/********************************************************************************/


static void mauto_do_change_speed(ModeAuto* mode_auto, const mission_command_t cmd)
{
    mode_base_t mode = &mode_auto->mode;

    if (cmd->content.speed.target_ms > 0) {
        if (cmd->content.speed.speed_type == 2)  {
            wpnav_set_speed_up(mode->wp_nav, cmd->content.speed.target_ms * 100.0f);
        } else if (cmd->content.speed.speed_type == 3)  {
            wpnav_set_speed_down(mode->wp_nav, cmd->content.speed.target_ms * 100.0f);
        } else {
            wpnav_set_speed_xy(mode->wp_nav, cmd->content.speed.target_ms * 100.0f);
        }
    }
}

static void mauto_do_set_home(ModeAuto* mode_auto, const mission_command_t cmd)
{
    if (cmd->p1 == 1 || (cmd->content.location.lat == 0 && cmd->content.location.lng == 0 && cmd->content.location.alt == 0)) {
        if (!fms_set_home_to_current_location(false)) {
            // ignore failure
        }
    } else {
        if (!fms_set_home(&cmd->content.location, false)) {
            // ignore failure
        }
    }
}

// do_roi - starts actions required by MAV_CMD_DO_SET_ROI
//          this involves either moving the camera to point at the ROI (region of interest)
//          and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
static void mauto_do_roi(ModeAuto* mode_auto, const mission_command_t cmd)
{
    autoyaw_set_roi(mode_auto->mode.auto_yaw, &cmd->content.location);
}

/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/

// verify_takeoff - check if we have completed the takeoff
static bool mauto_verify_takeoff(mode_base_t mode)
{
#if 0
#if LANDING_GEAR_ENABLED == ENABLED
    // if we have reached our destination
    if (auto_takeoff_complete) {
        // retract the landing gear
        copter.landinggear.retract_after_takeoff();
    }
#endif
#endif

    return (*mode->auto_takeoff_complete);
}

// verify_nav_wp - check if we have reached the next way point
static bool mauto_verify_nav_wp(mode_base_t mode, const mission_command_t cmd)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    // check if we have reached the waypoint
    if ( !wpnav_reached_wp_destination(mode->wp_nav) ) {
        return false;
    }

    // start timer if necessary
    if (mode_auto->loiter_time == 0) {
        mode_auto->loiter_time = time_millis();
        if (mode_auto->loiter_time_max > 0) {
            // play a tone
            notify_events.waypoint_complete = 1;
        }
    }

    // check if timer has run out
    if (((time_millis() - mode_auto->loiter_time) / 1000) >= mode_auto->loiter_time_max) {
        if (mode_auto->loiter_time_max == 0) {
            // play a tone
            notify_events.waypoint_complete = 1;
        }
        gcs_send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd->index);
        return true;
    }
    return false;
}

// verify_land - returns true if landing has been completed
static bool mauto_verify_land(mode_base_t mode)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    bool retval = false;

    switch (mode_auto->state) {
        case AutoFlyToLocation:
            // check if we've reached the location
            if (wpnav_reached_wp_destination(mode->wp_nav)) {
                // initialise landing controller
                mauto_land_start(mode_auto);

                // advance to next state
                mode_auto->state = AutoDescending;
            }
            break;

        case AutoDescending:
            // rely on THROTTLE_LAND mode to correctly update landing status
            retval = fms.ap.land_complete && (Motors_get_spool_state(mode->motors) == MOTOR_GROUND_IDLE);
            if (retval && !mission_continue_after_land_check_for_takeoff() && mode->motors->_armed) {
                /*
                  we want to stop mission processing on land
                  completion. Disarm now, then return false. This
                  leaves mission state machine in the current NAV_LAND
                  mission item. After disarming the mission will reset
                */
                mb_arming_disarm(ARMING_METHOD_LANDED, true);
                retval = false;
            }
            break;

        default:
            // this should never happen
            //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
            retval = true;
            break;
    }

    // true is returned if we've successfully landed
    return retval;
}

// verify_RTL - handles any state changes required to implement RTL
// do_RTL should have been called once first to initialise all variables
// returns true with RTL has completed successfully
static bool mauto_verify_RTL(mode_base_t mode)
{
    return (mrtl_state_complete((mode_base_t)&fms.mode_rtl) && 
            (fms.mode_rtl._state == RTL_FINAL_DESCENT || fms.mode_rtl._state == RTL_LAND) &&
            (Motors_get_spool_state(mode->motors) == MOTOR_GROUND_IDLE));
}

static bool mauto_verify_loiter_unlimited(mode_base_t mode)
{
    return false;
}

// verify_loiter_time - check if we have loitered long enough
static bool mauto_verify_loiter_time(mode_base_t mode, const mission_command_t cmd)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    // return immediately if we haven't reached our destination
    if (!wpnav_reached_wp_destination(mode->wp_nav)) {
        return false;
    }

    // start our loiter timer
    if (mode_auto->loiter_time == 0 ) {
        mode_auto->loiter_time = time_millis();
    }

    // check if loiter timer has run out
    if (((time_millis() - mode_auto->loiter_time) / 1000) >= mode_auto->loiter_time_max) {
        gcs_send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd->index);
        return true;
    }

    return false;
}

// verify_loiter_to_alt - check if we have reached both destination
// (roughly) and altitude (precisely)
static bool mauto_verify_loiter_to_alt(mode_base_t mode)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    if (mode_auto->loiter_to_alt.reached_destination_xy &&
        mode_auto->loiter_to_alt.reached_alt) {
        return true;
    }
    return false;
}

// verify_spline_wp - check if we have reached the next way point using spline
static bool mauto_verify_spline_wp(mode_base_t mode, const mission_command_t cmd)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    // check if we have reached the waypoint
    if ( !wpnav_reached_wp_destination(mode->wp_nav) ) {
        return false;
    }

    // start timer if necessary
    if (mode_auto->loiter_time == 0) {
        mode_auto->loiter_time = time_millis();
    }

    // check if timer has run out
    if (((time_millis() - mode_auto->loiter_time) / 1000) >= mode_auto->loiter_time_max) {
        gcs_send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd->index);
        return true;
    }
    return false;
}

#if NAV_GUIDED == ENABLED
// verify_nav_guided - check if we have breached any limits
static bool mauto_verify_nav_guided_enable(mode_base_t mode, const mission_command_t cmd)
{
    // if disabling guided mode then immediately return true so we move to next command
    if (cmd->p1 == 0) {
        return true;
    }

    // check time and position limits
    return mguided_limit_check();
}
#endif  // NAV_GUIDED

// verify_nav_delay - check if we have waited long enough
static bool mauto_verify_nav_delay(mode_base_t mode, const mission_command_t cmd)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    if (time_millis() - mode_auto->nav_delay_time_start_ms > mode_auto->nav_delay_time_max_ms) {
        mode_auto->nav_delay_time_max_ms = 0;
        return true;
    }
    return false;
}

// check if nav_attitude_time command has completed
static bool mauto_verify_nav_attitude_time(mode_base_t mode, const mission_command_t cmd)
{
    return ((time_millis() - ((ModeAuto*)mode)->nav_attitude_time.start_ms) > (cmd->content.nav_attitude_time.time_sec * 1000));
}

static bool mauto_verify_wait_delay(mode_base_t mode)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    if (time_millis() - mode_auto->condition_start > (uint32_t)MAX(mode_auto->condition_value,0)) {
        mode_auto->condition_value = 0;
        return true;
    }
    return false;
}

static bool mauto_verify_within_distance(mode_base_t mode)
{
    ModeAuto* mode_auto = (ModeAuto*)mode;

    if (mode_wp_distance(mode) < (uint32_t)MAX(mode_auto->condition_value,0)) {
        mode_auto->condition_value = 0;
        return true;
    }
    return false;
}

// verify_yaw - return true if we have reached the desired heading
static bool mauto_verify_yaw(mode_base_t mode)
{
    // make sure still in fixed yaw mode, the waypoint controller often retakes control of yaw as it executes a new waypoint command
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_FIXED);

    // check if we are within 2 degrees of the target heading
    return autoyaw_fixed_yaw_slew_finished(mode->auto_yaw) && (fabsf(math_wrap_180_cd(mode->ahrs->yaw_sensor_cd-autoyaw_yaw(mode->auto_yaw))) <= 200);
}

/*------------------------------------test------------------------------------*/


